sensor_fusion.h File Reference

Include dependency graph for sensor_fusion.h:

This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Data Structures
struct	PhysicalSensor
struct	PressureSensor
struct	AccelSensor
struct	MagSensor
struct	GyroSensor
union	FifoSensor
struct	SV_1DOF_P_BASIC
struct	SV_3DOF_G_BASIC
struct	SV_3DOF_B_BASIC
struct	SV_3DOF_Y_BASIC
struct	SV_6DOF_GB_BASIC
struct	SV_6DOF_GY_KALMAN
struct	SV_9DOF_GBY_KALMAN
struct	SV_COMMON
struct	SensorFusionGlobals

Macros
#define	CHZ   2
#define	true   1
#define	false   0
#define	GTOMSEC2   9.80665
#define	SPI_ADDR   0x00
Vector Components
Index values for accessing vector terms
#define	CHX   0
#define	CHY   1
Generic bit-field values
Generic bit-field values
#define	B0   (1 << 0)
#define	B1   (1 << 1)
#define	B2   (1 << 2)
#define	B3   (1 << 3)
Math Constants
useful multiplicative conversion constants
#define	PI   3.141592654F
#define	PIOVER2   1.570796327F
#define	FPIOVER180   0.01745329251994F
#define	F180OVERPI   57.2957795130823F
#define	F180OVERPISQ   3282.8063500117F
#define	ONETHIRD   0.33333333F
#define	ONESIXTH   0.166666667F
#define	ONESIXTEENTH   0.0625F
#define	ONEOVER12   0.083333333F
#define	ONEOVER48   0.02083333333F
#define	ONEOVER120   0.0083333333F
#define	ONEOVER3840   0.0002604166667F
#define	ONEOVERSQRT2   0.707106781F
#define	SQRT15OVER4   0.968245837F

Typedefs
typedef enum quaternion	quaternion_type
typedef int8_t(	initializeSensor_t) (struct PhysicalSensor *sensor, struct SensorFusionGlobals *sfg)
typedef int8_t(	readSensor_t) (struct PhysicalSensor *sensor, struct SensorFusionGlobals *sfg)
typedef int8_t(	readSensors_t) (struct SensorFusionGlobals *sfg, uint16_t read_loop_counter)
typedef int8_t(	installSensor_t) (struct SensorFusionGlobals *sfg, struct PhysicalSensor *sensor, uint16_t addr, uint16_t schedule, void *bus_driver, initializeSensor_t *initialize, readSensor_t *read)
typedef void(	initializeFusionEngine_t) (struct SensorFusionGlobals *sfg)
typedef void(	runFusion_t) (struct SensorFusionGlobals *sfg)
typedef void(	clearFIFOs_t) (struct SensorFusionGlobals *sfg)
typedef void(	conditionSensorReadings_t) (struct SensorFusionGlobals *sfg)
typedef void(	applyPerturbation_t) (struct SensorFusionGlobals *sfg)
typedef void(	setStatus_t) (struct SensorFusionGlobals *sfg, fusion_status_t status)
typedef void(	updateStatus_t) (struct SensorFusionGlobals *sfg)
typedef void(	ssSetStatus_t) (struct StatusSubsystem *pStatus, fusion_status_t status)
typedef void(	ssUpdateStatus_t) (struct StatusSubsystem *pStatus)
typedef struct PhysicalSensor	PhysicalSensor
typedef struct PressureSensor	PressureSensor
typedef struct AccelSensor	AccelSensor
typedef struct MagSensor	MagSensor
typedef struct GyroSensor	GyroSensor
typedef union FifoSensor	FifoSensor
typedef struct SV_1DOF_P_BASIC	SV_1DOF_P_BASIC
typedef struct SV_3DOF_G_BASIC	SV_3DOF_G_BASIC
typedef struct SV_3DOF_B_BASIC	SV_3DOF_B_BASIC
typedef struct SV_3DOF_Y_BASIC	SV_3DOF_Y_BASIC
typedef struct SV_6DOF_GB_BASIC	SV_6DOF_GB_BASIC
typedef struct SV_6DOF_GY_KALMAN	SV_6DOF_GY_KALMAN
typedef struct SV_9DOF_GBY_KALMAN	SV_9DOF_GBY_KALMAN
typedef struct SV_COMMON	SV_COMMON
typedef SV_COMMON *	SV_ptr
typedef struct SensorFusionGlobals	SensorFusionGlobals
Integer Typedefs
Typedefs to map common integer types to standard form
typedef unsigned char	byte
typedef int8_t	int8
typedef int16_t	int16
typedef int32_t	int32
typedef uint8_t	uint8
typedef uint16_t	uint16
typedef uint32_t	uint32

Enumerations
enum	quaternion {   Q3, Q3M, Q3G, Q6MA,   Q6AG, Q9 }
enum	fusion_status_t {   OFF, INITIALIZING, LOWPOWER, NORMAL,   RECEIVING_WIRED, RECEIVING_WIRELESS, HARD_FAULT, SOFT_FAULT }

Functions
void	initSensorFusionGlobals (SensorFusionGlobals *sfg, struct StatusSubsystem *pStatusSubsystem, struct ControlSubsystem *pControlSubsystem)
void	conditionSensorReadings (SensorFusionGlobals *sfg)
void	clearFIFOs (SensorFusionGlobals *sfg)
void	zeroArray (struct StatusSubsystem *pStatus, void *data, uint16_t size, uint16_t numElements, uint8_t check)
void	conditionSample (int16_t sample[3])
void	addToFifo (FifoSensor *sensor, uint16_t maxFifoSize, int16_t sample[3])
void	ApplyAccelHAL (AccelSensor *Accel)
void	ApplyMagHAL (MagSensor *Mag)
void	ApplyGyroHAL (GyroSensor *Gyro)

Variables
installSensor_t	installSensor
initializeFusionEngine_t	initializeFusionEngine
runFusion_t	runFusion
readSensors_t	readSensors
applyPerturbation_t	ApplyPerturbation

Macro Definition Documentation

#define B0   (1 << 0)

Definition at line 88 of file sensor_fusion.h.

Referenced by fusion_task(), and read_task().

#define B1   (1 << 1)

Definition at line 89 of file sensor_fusion.h.

#define B2   (1 << 2)

Definition at line 90 of file sensor_fusion.h.

#define B3   (1 << 3)

Definition at line 91 of file sensor_fusion.h.

#define CHX   0

Used to access X-channel entries in various data data structures.

Definition at line 76 of file sensor_fusion.h.

Referenced by addToFifo(), ApplyAccelHAL(), ApplyGyroHAL(), ApplyMagHAL(), conditionSample(), CreateAndSendPackets(), DecodeCommandBytes(), f3DOFMagnetometerMatrixAndroid(), f3DOFMagnetometerMatrixNED(), f3x3matrixDetA(), fAndroidAnglesDegFromRotationMatrix(), fComputeAccelCalibration10(), fComputeAccelCalibration4(), fComputeAccelCalibration7(), fComputeMagCalibration10(), fComputeMagCalibration4(), fComputeMagCalibration7(), feCompassAndroid(), feCompassNED(), feCompassWin8(), fInit_6DOF_GY_KALMAN(), fInit_9DOF_GBY_KALMAN(), fInitializeAccelCalibration(), fInitializeMagCalibration(), fInvertAccelCal(), fInvertMagCal(), fLPFOrientationQuaternion(), fNEDAnglesDegFromRotationMatrix(), fQuaternionFromRotationMatrix(), fQuaternionFromRotationVectorDeg(), fRotationMatrixFromQuaternion(), fRotationVectorDegFromQuaternion(), fRun_3DOF_Y_BASIC(), fRun_6DOF_GY_KALMAN(), fRunAccelCalibration(), fRunMagCalibration(), fUpdateAccelBuffer(), fUpdateMagCalibration10Slice(), fUpdateMagCalibration4Slice(), fUpdateMagCalibration7Slice(), fVeq3x3AxV(), fveqconjgquq(), fveqRu(), fWin8AnglesDegFromRotationMatrix(), FXLS8471Q_Read(), FXOS8700_Init(), FXOS8700_ReadMagData(), initializeSensors(), iUpdateMagBuffer(), MAG3110_Read(), motionCheck(), processMagData(), and readCommon().

#define CHY   1

Used to access Y-channel entries in various data data structures.

Definition at line 77 of file sensor_fusion.h.

Referenced by addToFifo(), ApplyAccelHAL(), ApplyGyroHAL(), ApplyMagHAL(), conditionSample(), CreateAndSendPackets(), f3DOFMagnetometerMatrixAndroid(), f3DOFMagnetometerMatrixNED(), f3x3matrixDetA(), fAndroidAnglesDegFromRotationMatrix(), fComputeAccelCalibration10(), fComputeAccelCalibration4(), fComputeAccelCalibration7(), fComputeMagCalibration10(), fComputeMagCalibration4(), fComputeMagCalibration7(), feCompassAndroid(), feCompassNED(), feCompassWin8(), fInitializeAccelCalibration(), fInitializeMagCalibration(), fInvertAccelCal(), fInvertMagCal(), fLPFOrientationQuaternion(), fNEDAnglesDegFromRotationMatrix(), fQuaternionFromRotationMatrix(), fQuaternionFromRotationVectorDeg(), fRotationMatrixFromQuaternion(), fRotationVectorDegFromQuaternion(), fRun_6DOF_GY_KALMAN(), fUpdateMagCalibration10Slice(), fUpdateMagCalibration4Slice(), fUpdateMagCalibration7Slice(), fVeq3x3AxV(), fveqconjgquq(), fveqRu(), fWin8AnglesDegFromRotationMatrix(), FXLS8471Q_Read(), FXOS8700_Init(), FXOS8700_ReadMagData(), iUpdateMagBuffer(), MAG3110_Read(), motionCheck(), and readCommon().

#define CHZ   2

Referenced by addToFifo(), ApplyAccelHAL(), ApplyGyroHAL(), ApplyMagHAL(), conditionSample(), CreateAndSendPackets(), DecodeCommandBytes(), f3DOFMagnetometerMatrixAndroid(), f3DOFMagnetometerMatrixNED(), f3x3matrixDetA(), fAndroidAnglesDegFromRotationMatrix(), fComputeAccelCalibration10(), fComputeAccelCalibration4(), fComputeAccelCalibration7(), fComputeMagCalibration10(), fComputeMagCalibration4(), fComputeMagCalibration7(), feCompassAndroid(), feCompassNED(), feCompassWin8(), fInit_6DOF_GY_KALMAN(), fInit_9DOF_GBY_KALMAN(), fInitializeAccelCalibration(), fInitializeMagCalibration(), fInvertAccelCal(), fInvertMagCal(), fLPFOrientationQuaternion(), fNEDAnglesDegFromRotationMatrix(), fQuaternionFromRotationMatrix(), fQuaternionFromRotationVectorDeg(), fRotationMatrixFromQuaternion(), fRotationVectorDegFromQuaternion(), fRun_3DOF_Y_BASIC(), fRun_6DOF_GY_KALMAN(), fRunAccelCalibration(), fRunMagCalibration(), fUpdateAccelBuffer(), fUpdateMagCalibration10Slice(), fUpdateMagCalibration4Slice(), fUpdateMagCalibration7Slice(), fVeq3x3AxV(), fveqconjgquq(), fveqRu(), fWin8AnglesDegFromRotationMatrix(), FXLS8471Q_Read(), FXOS8700_Init(), FXOS8700_ReadMagData(), initializeSensors(), iUpdateMagBuffer(), MAG3110_Read(), motionCheck(), processMagData(), and readCommon().

#define F180OVERPI   57.2957795130823F

radians to degrees conversion = 180 / pi

Definition at line 100 of file sensor_fusion.h.

Referenced by ApplyPerturbation(), and fRotationVectorDegFromQuaternion().

#define F180OVERPISQ   3282.8063500117F

square of F180OVERPI

Definition at line 101 of file sensor_fusion.h.

#define false   0

Boolean FALSE.

Definition at line 83 of file sensor_fusion.h.

#define FPIOVER180   0.01745329251994F

degrees to radians conversion = pi / 180

Definition at line 99 of file sensor_fusion.h.

Referenced by fInit_6DOF_GY_KALMAN(), fInit_9DOF_GBY_KALMAN(), fQuaternionFromRotationVectorDeg(), and fRotationVectorDegFromQuaternion().

#define GTOMSEC2   9.80665

Referenced by fInit_9DOF_GBY_KALMAN().

#define ONEOVER12   0.083333333F

1 / 12

Definition at line 105 of file sensor_fusion.h.

Referenced by fRun_6DOF_GY_KALMAN().

#define ONEOVER120   0.0083333333F

1 / 120

Definition at line 107 of file sensor_fusion.h.

#define ONEOVER3840   0.0002604166667F

1 / 3840

Definition at line 108 of file sensor_fusion.h.

Referenced by fQuaternionFromRotationVectorDeg().

#define ONEOVER48   0.02083333333F

1 / 48

Definition at line 106 of file sensor_fusion.h.

Referenced by fQuaternionFromRotationVectorDeg().

#define ONEOVERSQRT2   0.707106781F

1/sqrt(2)

Definition at line 109 of file sensor_fusion.h.

Referenced by ApplyPerturbation(), and fveqconjgquq().

#define ONESIXTEENTH   0.0625F

one sixteenth

Definition at line 104 of file sensor_fusion.h.

#define ONESIXTH   0.166666667F

one sixth

Definition at line 103 of file sensor_fusion.h.

Referenced by fComputeMagCalibration10(), and fComputeMagCalibration7().

#define ONETHIRD   0.33333333F

one third

Definition at line 102 of file sensor_fusion.h.

Referenced by fComputeMagCalibration10(), fComputeMagCalibration7(), fUpdateMagCalibration10Slice(), and fUpdateMagCalibration7Slice().

#define PI   3.141592654F

pi

Definition at line 97 of file sensor_fusion.h.

Referenced by fInitializeMagCalibration().

#define PIOVER2   1.570796327F

pi / 2

Definition at line 98 of file sensor_fusion.h.

#define SPI_ADDR   0x00

Definition at line 153 of file sensor_fusion.h.

Referenced by FXLS8471Q_Idle(), FXLS8471Q_Init(), and FXLS8471Q_Read().

#define SQRT15OVER4   0.968245837F

sqrt(15)/4

Definition at line 110 of file sensor_fusion.h.

#define true   1

Boolean TRUE.

Definition at line 82 of file sensor_fusion.h.

Typedef Documentation

typedef struct AccelSensor AccelSensor

The AccelSensor structure stores raw and processed measurements for a 3-axis accelerometer.

The AccelSensor structure stores raw and processed measurements, as well as metadata for a single 3-axis accelerometer. This structure is normally "fed" by the sensor driver and "consumed" by the fusion routines.

typedef void( applyPerturbation_t) (struct SensorFusionGlobals *sfg)

Definition at line 160 of file sensor_fusion.h.

typedef unsigned char byte

Definition at line 54 of file sensor_fusion.h.

typedef void( clearFIFOs_t) (struct SensorFusionGlobals *sfg)

Definition at line 158 of file sensor_fusion.h.

typedef void( conditionSensorReadings_t) (struct SensorFusionGlobals *sfg)

Definition at line 159 of file sensor_fusion.h.

typedef union FifoSensor FifoSensor

The FifoSensor union allows us to use common pointers for Accel, Mag & Gyro logical sensor structures.

Common elements include: iWhoAmI, isEnabled, iFIFOCount, iFIFOExceeded and the FIFO itself.

typedef struct GyroSensor GyroSensor

The GyroSensor structure stores raw and processed measurements for a 3-axis gyroscope.

The GyroSensor structure stores raw and processed measurements, as well as metadata for a single 3-axis gyroscope. This structure is normally "fed" by the sensor driver and "consumed" by the fusion routines.

typedef void( initializeFusionEngine_t) (struct SensorFusionGlobals *sfg)

Definition at line 156 of file sensor_fusion.h.

typedef int8_t( initializeSensor_t) (struct PhysicalSensor *sensor, struct SensorFusionGlobals *sfg)

Definition at line 132 of file sensor_fusion.h.

typedef int8_t( installSensor_t) (struct SensorFusionGlobals *sfg, struct PhysicalSensor *sensor, uint16_t addr, uint16_t schedule, void *bus_driver, initializeSensor_t *initialize, readSensor_t *read)

Definition at line 144 of file sensor_fusion.h.

typedef int16_t int16

Definition at line 56 of file sensor_fusion.h.

typedef int32_t int32

Definition at line 57 of file sensor_fusion.h.

typedef int8_t int8

Definition at line 55 of file sensor_fusion.h.

typedef struct MagSensor MagSensor

The MagSensor structure stores raw and processed measurements for a 3-axis magnetic sensor.

The MagSensor structure stores raw and processed measurements, as well as metadata for a single 3-axis magnetometer. This structure is normally "fed" by the sensor driver and "consumed" by the fusion routines.

typedef struct PhysicalSensor PhysicalSensor

An instance of PhysicalSensor structure type should be allocated for each physical sensors (combo devices = 1)

These structures sit 'on-top-of' the pre-7.0 sensor fusion structures and give us the ability to do run time driver installation.

typedef struct PressureSensor PressureSensor

The PressureSensor structure stores raw and processed measurements for an altimeter.

The PressureSensor structure stores raw and processed measurements, as well as metadata for a pressure sensor/altimeter.

typedef enum quaternion quaternion_type

the quaternion type to be transmitted

typedef int8_t( readSensor_t) (struct PhysicalSensor *sensor, struct SensorFusionGlobals *sfg)

Definition at line 136 of file sensor_fusion.h.

typedef int8_t( readSensors_t) (struct SensorFusionGlobals *sfg, uint16_t read_loop_counter)

Definition at line 140 of file sensor_fusion.h.

typedef void( runFusion_t) (struct SensorFusionGlobals *sfg)

Definition at line 157 of file sensor_fusion.h.

typedef struct SensorFusionGlobals SensorFusionGlobals

The top level fusion structure.

The top level fusion structure grows/shrinks based upon flag definitions contained in build.h. These same flags will populate the .iFlags field for run-time access.

typedef void( setStatus_t) (struct SensorFusionGlobals *sfg, fusion_status_t status)

Definition at line 161 of file sensor_fusion.h.

typedef void( ssSetStatus_t) (struct StatusSubsystem *pStatus, fusion_status_t status)

Definition at line 163 of file sensor_fusion.h.

typedef void( ssUpdateStatus_t) (struct StatusSubsystem *pStatus)

Definition at line 164 of file sensor_fusion.h.

typedef struct SV_1DOF_P_BASIC SV_1DOF_P_BASIC

The SV_1DOF_P_BASIC structure contains state information for a pressure sensor/altimeter.

typedef struct SV_3DOF_B_BASIC SV_3DOF_B_BASIC

This is the 3DOF basic magnetometer state vector structure/.

typedef struct SV_3DOF_G_BASIC SV_3DOF_G_BASIC

This is the 3DOF basic accelerometer state vector structure.

typedef struct SV_3DOF_Y_BASIC SV_3DOF_Y_BASIC

SV_3DOF_Y_BASIC structure is the 3DOF basic gyroscope state vector structure.

typedef struct SV_6DOF_GB_BASIC SV_6DOF_GB_BASIC

SV_6DOF_GB_BASIC is the 6DOF basic accelerometer and magnetometer state vector structure.

typedef struct SV_6DOF_GY_KALMAN SV_6DOF_GY_KALMAN

SV_6DOF_GY_KALMAN is the 6DOF Kalman filter accelerometer and gyroscope state vector structure.

typedef struct SV_9DOF_GBY_KALMAN SV_9DOF_GBY_KALMAN

SV_9DOF_GBY_KALMAN is the 9DOF Kalman filter accelerometer, magnetometer and gyroscope state vector structure.

typedef struct SV_COMMON SV_COMMON

Excluding SV_1DOF_P_BASIC, Any of the SV_ fusion structures above could be cast to type SV_COMMON for dereferencing.

typedef SV_COMMON* SV_ptr

Definition at line 460 of file sensor_fusion.h.

typedef uint16_t uint16

Definition at line 59 of file sensor_fusion.h.

typedef uint32_t uint32

Definition at line 60 of file sensor_fusion.h.

typedef uint8_t uint8

Definition at line 58 of file sensor_fusion.h.

typedef void( updateStatus_t) (struct SensorFusionGlobals *sfg)

Definition at line 162 of file sensor_fusion.h.

Enumeration Type Documentation

enum fusion_status_t

Application-specific serial communications system.

Enumerator
OFF	These are the state definitions for the status subsystem. Application hasn't started
INITIALIZING	Initializing sensors and algorithms.
LOWPOWER	Running in reduced power mode.
NORMAL	Operation is Nominal.
RECEIVING_WIRED	Receiving commands over wired interface (momentary)
RECEIVING_WIRELESS	Receiving commands over wireless interface (momentary)
HARD_FAULT	Non-recoverable FAULT = something went very wrong.
SOFT_FAULT	Recoverable FAULT = something went wrong, but we can keep going.

Definition at line 120 of file sensor_fusion.h.

             {                                  ///  These are the state definitions for the status subsystem
    OFF,                                    ///< Application hasn't started
    INITIALIZING,                           ///< Initializing sensors and algorithms
    LOWPOWER,                               ///< Running in reduced power mode
    NORMAL,                                 ///< Operation is Nominal
        RECEIVING_WIRED,                        ///< Receiving commands over wired interface (momentary)
        RECEIVING_WIRELESS,                     ///< Receiving commands over wireless interface (momentary)
    HARD_FAULT,                             ///< Non-recoverable FAULT = something went very wrong
        SOFT_FAULT                              ///< Recoverable FAULT = something went wrong, but we can keep going
} fusion_status_t;

enum quaternion

the quaternion type to be transmitted

Enumerator
Q3	Quaternion derived from 3-axis accel (tilt)
Q3M	Quaternion derived from 3-axis mag only (auto compass algorithm)
Q3G	Quaternion derived from 3-axis gyro only (rotation)
Q6MA	Quaternion derived from 3-axis accel + 3 axis mag (eCompass)
Q6AG	Quaternion derived from 3-axis accel + 3-axis gyro (gaming)
Q9	Quaternion derived from full 9-axis sensor fusion.

Definition at line 64 of file sensor_fusion.h.

                        {
    Q3,         ///< Quaternion derived from 3-axis accel (tilt)
    Q3M,        ///< Quaternion derived from 3-axis mag only (auto compass algorithm)
    Q3G,        ///< Quaternion derived from 3-axis gyro only (rotation)
    Q6MA,       ///< Quaternion derived from 3-axis accel + 3 axis mag (eCompass)
    Q6AG,       ///< Quaternion derived from 3-axis accel + 3-axis gyro (gaming)
    Q9          ///< Quaternion derived from full 9-axis sensor fusion
} quaternion_type;

Function Documentation

void addToFifo	(	FifoSensor *	sensor,
		uint16_t	maxFifoSize,
		int16_t	sample[3]
	)

addToFifo is called from within sensor driver read functions

addToFifo is called from within sensor driver read functions to transfer new readings into the sensor structure corresponding to accel, gyro or mag. This function ensures that the software FIFOs are not overrun.

example usage: if (status==SENSOR_ERROR_NONE) addToFifo((FifoSensor*) &(sfg->Mag), MAG_FIFO_SIZE, sample);

Parameters

sensor	pointer to structure of type AccelSensor, MagSensor or GyroSensor
maxFifoSize	the size of the software (not hardware) FIFO
sample	the sample to add

Definition at line 531 of file sensor_fusion.c.

Referenced by FXLS8471Q_Read(), FXOS8700_Init(), FXOS8700_ReadMagData(), and MAG3110_Read().

{
  // Note that FifoSensor is a union of GyroSensor, MagSensor and AccelSensor.
  // All contain FIFO structures in the same location.  We use the Accel
  // structure to index here.

  // example usage: if (status==SENSOR_ERROR_NONE) addToFifo((FifoSensor*) &(sfg->Mag), MAG_FIFO_SIZE, sample);
    uint8_t fifoCount = sensor->Accel.iFIFOCount;
    if (fifoCount < maxFifoSize) {
        // we have room for the new sample
        sensor->Accel.iGsFIFO[fifoCount][CHX] = sample[CHX];
        sensor->Accel.iGsFIFO[fifoCount][CHY] = sample[CHY];
        sensor->Accel.iGsFIFO[fifoCount][CHZ] = sample[CHZ];
        sensor->Accel.iFIFOCount += 1;
        sensor->Accel.iFIFOExceeded = 0;
    } else {
        //there is no room for a new sample
        sensor->Accel.iFIFOExceeded += 1;
    }
}

Here is the caller graph for this function:

void ApplyAccelHAL

(

AccelSensor *

Accel

)

Apply the accelerometer Hardware Abstraction Layer.

Parameters

Accel

pointer to accelerometer logical sensor

Definition at line 44 of file hal_frdm_fxs_mult2_b.c.

Referenced by initializeSensors().

{
    int8 i;             // loop counter

    // apply HAL to all measurements read from FIFO buffer
    for (i = 0; i < Accel->iFIFOCount; i++)
    {
        // apply HAL mapping to coordinate system used
#if THISCOORDSYSTEM == NED
        int16 itmp16 = Accel->iGsFIFO[i][CHX];
        Accel->iGsFIFO[i][CHX] = Accel->iGsFIFO[i][CHY];
        Accel->iGsFIFO[i][CHY] = itmp16;
#endif // NED
#if THISCOORDSYSTEM == ANDROID
        Accel->iGsFIFO[i][CHX] = -Accel->iGsFIFO[i][CHX];
        Accel->iGsFIFO[i][CHY] = -Accel->iGsFIFO[i][CHY];
#endif // Android
#if (THISCOORDSYSTEM == WIN8)
        Accel->iGsFIFO[i][CHZ] = -Accel->iGsFIFO[i][CHZ];
#endif // Win8

    } // end of loop over FIFO count

    return;
}

Here is the caller graph for this function:

void ApplyGyroHAL

(

GyroSensor *

Gyro

)

Apply the gyroscope Hardware Abstraction Layer.

Parameters

Gyro	pointer to gyroscope logical sensor

Definition at line 99 of file hal_frdm_fxs_mult2_b.c.

Referenced by processMagData().

{
    int8 i;             // loop counter

    // apply HAL to all measurements read from FIFO buffer
    for (i = 0; i < Gyro->iFIFOCount; i++)
    {
        // apply HAL mapping to coordinate system used
#if THISCOORDSYSTEM == NED
        int16 itmp16 = Gyro->iYsFIFO[i][CHX];
        Gyro->iYsFIFO[i][CHX] = -Gyro->iYsFIFO[i][CHY];
        Gyro->iYsFIFO[i][CHY] = -itmp16;
        Gyro->iYsFIFO[i][CHZ] = -Gyro->iYsFIFO[i][CHZ];
#endif // NED
#if THISCOORDSYSTEM == ANDROID
        Gyro->iYsFIFO[i][CHX] = -Gyro->iYsFIFO[i][CHX];
        Gyro->iYsFIFO[i][CHY] = -Gyro->iYsFIFO[i][CHY];
#endif // Android
#if THISCOORDSYSTEM == WIN8
        Gyro->iYsFIFO[i][CHX] = -Gyro->iYsFIFO[i][CHX];
        Gyro->iYsFIFO[i][CHY] = -Gyro->iYsFIFO[i][CHY];
#endif // Win8

    } // end of loop over FIFO count

    return;
}

Here is the caller graph for this function:

void ApplyMagHAL

(

MagSensor *

Mag

)

Apply the magnetometer Hardware Abstraction Layer.

Parameters

Mag	pointer to magnetometer logical sensor

Definition at line 71 of file hal_frdm_fxs_mult2_b.c.

Referenced by processMagData().

{
    int8 i;             // loop counter

    // apply HAL to all measurements read from FIFO buffer
    for (i = 0; i < Mag->iFIFOCount; i++)
    {
        // apply HAL mapping to coordinate system used
#if THISCOORDSYSTEM == NED
        int16 itmp16 = Mag->iBsFIFO[i][CHX];
        Mag->iBsFIFO[i][CHX] = -Mag->iBsFIFO[i][CHY];
        Mag->iBsFIFO[i][CHY] = -itmp16;
        Mag->iBsFIFO[i][CHZ] = -Mag->iBsFIFO[i][CHZ];
#endif // NED
#if THISCOORDSYSTEM == ANDROID
        Mag->iBsFIFO[i][CHX] = -Mag->iBsFIFO[i][CHX];
        Mag->iBsFIFO[i][CHY] = -Mag->iBsFIFO[i][CHY];
#endif // Android
#if THISCOORDSYSTEM == WIN8
        Mag->iBsFIFO[i][CHX] = -Mag->iBsFIFO[i][CHX];
        Mag->iBsFIFO[i][CHY] = -Mag->iBsFIFO[i][CHY];
#endif
    } // end of loop over FIFO count

    return;
}

Here is the caller graph for this function:

void clearFIFOs

(

SensorFusionGlobals *

sfg

)

Function to clear FIFO at the end of each fusion computation.

Parameters

sfg	Global data structure pointer

Definition at line 363 of file sensor_fusion.c.

Referenced by initializeFusionEngine(), initSensorFusionGlobals(), and runFusion().

                                          {
  // We only clear FIFOs if the sensors are enabled.  This allows us
  // to continue to use these values when we've shut higher power consumption
  // sensors down during periods of no activity.
#if F_USING_ACCEL
    sfg->Accel.iFIFOCount=0;
    sfg->Accel.iFIFOExceeded = false;
#endif
#if F_USING_MAG
    sfg->Mag.iFIFOCount=0;
    sfg->Mag.iFIFOExceeded = false;
#endif
#if F_USING_GYRO
    sfg->Gyro.iFIFOCount=0;
    sfg->Gyro.iFIFOExceeded = false;
#endif
}

Here is the caller graph for this function:

void conditionSample

(

int16_t

sample[3]

)

conditionSample ensures that we never encounter the maximum negative two's complement value for a 16-bit variable (-32768).

conditionSample ensures that we never encounter the maximum negative two's complement value for a 16-bit variable (-32768). This value cannot be negated, because the maximum positive value is +32767. We need the ability to negate to gaurantee that subsequent HAL operations can be run successfully.

Parameters

sample

16-bit register value from triaxial sensor read

Definition at line 519 of file sensor_fusion.c.

Referenced by FXLS8471Q_Read(), FXOS8700_Init(), FXOS8700_ReadMagData(), and MAG3110_Read().

{
    // This function should be called for every 16 bit sample read from sensor hardware.
    // It is responsible for making sure that we never pass on the value of -32768.
    // That value cannot be properly negated using 16-bit twos complement math.
    // The ability to be later negated is required for general compatibility
    // with possible HAL (Hardware abstraction logic) which is run later in
    // the processing pipeline.
    if (sample[CHX] == -32768) sample[CHX]++;
    if (sample[CHY] == -32768) sample[CHY]++;
    if (sample[CHZ] == -32768) sample[CHZ]++;
}

Here is the caller graph for this function:

void conditionSensorReadings

(

SensorFusionGlobals *

sfg

)

conditionSensorReadings() transforms raw software FIFO readings into forms that can be consumed by the sensor fusion engine. This include sample averaging and (in the case of the gyro) integrations, applying hardware abstraction layers, and calibration functions. This function is normally involved via the "sfg." global pointer.

Parameters

sfg	Global data structure pointer

Definition at line 305 of file sensor_fusion.c.

Referenced by initSensorFusionGlobals().

                                                       {
#if F_USING_ACCEL
    if (sfg->Accel.isEnabled) processAccelData(sfg);
#endif

#if F_USING_MAG
    if (sfg->Mag.isEnabled) processMagData(sfg);
#endif

#if F_USING_GYRO
    if (sfg->Gyro.isEnabled) processGyroData(sfg);
#endif
    return;
}

Here is the call graph for this function:

Here is the caller graph for this function:

void initSensorFusionGlobals	(	SensorFusionGlobals *	sfg,
		struct StatusSubsystem *	pStatusSubsystem,
		struct ControlSubsystem *	pControlSubsystem
	)

utility function to insert default values in the top level structure

Parameters

sfg	Global data structure pointer
pStatusSubsystem	Status subsystem pointer
pControlSubsystem	Control subsystem pointer

Definition at line 68 of file sensor_fusion.c.

Referenced by main().

{
    sfg->iFlags = // all of the following defines are either 0x0000 or a 1-bit value (2, 4, 8 ...) and are defined in build.h
                F_USING_ACCEL           |
                F_USING_MAG             |
                F_USING_GYRO            |
                F_USING_PRESSURE        |
                F_USING_TEMPERATURE     |
                F_ALL_SENSORS           |       // refers to all applicable sensor types for the given physical unit
                F_1DOF_P_BASIC          |       // 1DOF pressure (altitude) and temperature: (pressure)
                F_3DOF_G_BASIC          |   // 3DOF accel tilt: (accel)
                F_3DOF_B_BASIC          |   // 3DOF mag eCompass (vehicle): (mag)
                F_3DOF_Y_BASIC          |   // 3DOF gyro integration: (gyro)
                F_6DOF_GB_BASIC         |   // 6DOF accel and mag eCompass)
                F_6DOF_GY_KALMAN        |   // 6DOF accel and gyro (Kalman): (accel + gyro)
                F_9DOF_GBY_KALMAN   ;   // 9DOF accel, mag and gyro (Kalman): (accel + mag + gyro)

    sfg->pControlSubsystem = pControlSubsystem;
    sfg->pStatusSubsystem = pStatusSubsystem;
    sfg->loopcounter = 0;                     // counter incrementing each iteration of sensor fusion (typically 25Hz)
    sfg->systick_I2C = 0;                     // systick counter to benchmark I2C reads
    sfg->systick_Spare = 0;                   // systick counter for counts spare waiting for timing interrupt
    sfg->iPerturbation = 0;                   // no perturbation to be applied
    sfg->installSensor = installSensor;       // function for installing a new sensor into the structures
    sfg->initializeFusionEngine = initializeFusionEngine;   // function for installing a new sensor into the structures
    sfg->readSensors = readSensors;           // function for installing a new sensor into the structures
    sfg->runFusion = runFusion;               // function for installing a new sensor into the structures
    sfg->applyPerturbation = ApplyPerturbation; // function used for step function testing
    sfg->conditionSensorReadings = conditionSensorReadings; // function does averaging, HAL adjustments, etc.
    sfg->clearFIFOs = clearFIFOs;             // function to clear FIFO flags    sfg->applyPerturbation = ApplyPerturbation; // function used for step function testing
    sfg->setStatus = setStatus;               // function to immediately set status change
    sfg->queueStatus = queueStatus;           // function to queue status change
    sfg->updateStatus = updateStatus;         // function to promote queued status change
    sfg->testStatus = testStatus;             // function for unit testing the status subsystem
    sfg->pSensors = NULL;                     // pointer to linked list of physical sensors
//  put error value into whoAmI as initial value
#if F_USING_ACCEL
    sfg->Accel.iWhoAmI = 0;
#endif
#if F_USING_MAG
    sfg->Mag.iWhoAmI = 0;
#endif
#if F_USING_GYRO
    sfg->Gyro.iWhoAmI = 0;
#endif
#if F_USING_PRESSURE
    sfg->Pressure.iWhoAmI = 0;
#endif
}

Here is the call graph for this function:

Here is the caller graph for this function:

void zeroArray	(	struct StatusSubsystem *	pStatus,
		void *	data,
		uint16_t	size,
		uint16_t	numElements,
		uint8_t	check
	)

Parameters

pStatus	Status subsystem pointer
data	pointer to array to be zeroed
size	data type size = 8, 16 or 32
numElements	number of elements to zero out
check	true if you would like to verify writes, false otherwise

Definition at line 320 of file sensor_fusion.c.

                                                                                                         {
  uint16_t i;
  uint8_t *d8;
  uint16_t *d16;
  uint32_t *d32;
  switch(size) {
  case 8:
    d8 = data;
    for (i=0; i<numElements; i++) d8[i]=0;
    break;
  case 16:
    d16 = data;
    for (i=0; i<numElements; i++) d16[i]=0;
    break;
  case 32:
    d32 = data;
    for (i=0; i<numElements; i++) d32[i]=0;
    break;
  default:
    pStatus->set(pStatus, HARD_FAULT);
  }
  if (check) {
    switch(size) {
    case 8:
      d8 = data;
      for (i=0; i<numElements; i++)
        if (d8[i]!=0) pStatus->set(pStatus, HARD_FAULT);
      break;
    case 16:
      d16 = data;
      for (i=0; i<numElements; i++)
        if (d16[i]!=0) pStatus->set(pStatus, HARD_FAULT);
      break;
    case 32:
      d32 = data;
      for (i=0; i<numElements; i++)
        if (d32[i]!=0) pStatus->set(pStatus, HARD_FAULT);
      break;
    }
    return;
  }
}

Variable Documentation

applyPerturbation_t ApplyPerturbation

ApplyPerturbation is a reverse unit-step test function.

The ApplyPerturbation function applies a user-specified step function to prior fusion results which is then "released" in the next fusion cycle. When used in conjuction with the NXP Sensor Fusion Toolbox, this provides a visual indication of the dynamic behavior of the library. ApplyPerturbation() is defined in debug.c.

Definition at line 626 of file sensor_fusion.h.

Referenced by initSensorFusionGlobals().

initializeFusionEngine_t initializeFusionEngine

Definition at line 558 of file sensor_fusion.h.

installSensor_t installSensor

Definition at line 557 of file sensor_fusion.h.

readSensors_t readSensors

Definition at line 571 of file sensor_fusion.h.

runFusion_t runFusion

Definition at line 570 of file sensor_fusion.h.